#ifndef _INTEGRATOR_H
#define _INTEGRATOR_H

#define OPTIMIZATION

/* Some physics constants */
#define DAMPING 0.01 // how much to damp the cloth simulation each frame

//#define TIME_STEPSIZE 0.5 // for velocity term
#define TIME_STEPSIZE 0.1

#define TIME_STEPSIZE2 0.25 //0.5*0.5 // how large time step each particle takes each frame (dt^2)
//#define CONSTRAINT_ITERATIONS 15 // how many iterations of constraint satisfaction each frame (more is rigid, less is soft)
#define CONSTRAINT_ITERATIONS 10 // how many iterations of constraint satisfaction each frame (more is rigid, less is soft)
//cloth2
//#define CONSTRAINT_ITERATIONS 10

#include <iostream>
#include <vector>
//#include "Particle.h"
#include "Constraint.h"
#include "Logger.h"

#include <omp.h>

enum IntegrationType{VERLET,POSITION_BASED_DYNAMICS};

/* Partial Differential Equation integrator. Use Strategy design pattern. */
class Integrator{
public:
	/* integrate a time step. */
	virtual void integrate(Real timeStep)=0;
};

///* Verlet Differential Equation integrator with implicit velocity scheme. */
//class ExplicitEulerIntegrator:public Integrator{
//public:
//	ExplicitEulerIntegrator(std::vector<Particle>& aparticles, std::vector<Constraint>& aconstraints):particles(aparticles),constraints(aconstraints){}
//
//protected:
//	std::vector<Particle>& particles;	// particles to integrate
//	std::vector<Constraint>& constraints;	// constraints to satisfy
//}

/* Verlet Differential Equation integrator with implicit velocity scheme. */
class VerletIntegrator:public Integrator{
public:
	VerletIntegrator(std::vector<Particle>& aparticles, std::vector<Constraint>& aconstraints):particles(aparticles),constraints(aconstraints){}
	void integrate(Real timeStep){
		std::vector<Particle>::iterator particleIter;
		std::vector<Constraint>::iterator constraintIter;
		Logger* logger = Logger::getInstance();

		logger->startLogTime("integration");
		for(particleIter = particles.begin(); particleIter != particles.end(); particleIter++)
		{
			Particle& particle = (*particleIter);
			if(particle.movable){
			//particle.timeStep(); // calculate the position of each particle at the next time step.
			Vector3 temp = particle.position;
			particle.newPosition = particle.position + (particle.position-particle.oldPosition)*(1.0-DAMPING) + particle.acceleration*TIME_STEPSIZE2;
			particle.oldPosition = temp;
			}
			else{
				particle.newPosition = particle.position;
				particle.oldPosition = particle.position;
			}
		}
		logger->endLogTime();

		logger->startLogTime("constraint");
		for(int i=0; i<CONSTRAINT_ITERATIONS; i++) // iterate over all constraints several times
		{
			for(constraintIter = constraints.begin(); constraintIter != constraints.end(); constraintIter++ )
			{
				(*constraintIter).satisfyConstraint(); // satisfy constraint.
			}
		}
		logger->endLogTime();

		logger->startLogTime("update");
		for(particleIter = particles.begin(); particleIter != particles.end(); particleIter++)
		{
			Particle& particle = (*particleIter);
			//if(particle.movable){
			particle.position = particle.newPosition;
			particle.resetAcceleration();
			//}
		}
		logger->endLogTime();
	}
protected:
	std::vector<Particle>& particles;	// particles to integrate
	std::vector<Constraint>& constraints;	// constraints to satisfy
};

/* Position-based Dynamics Equation integrator. */
class PositionBasedDynamicsIntegrator:public Integrator{
public:
	PositionBasedDynamicsIntegrator(std::vector<Particle>& aparticles, std::vector<Constraint>& aconstraints):particles(aparticles),constraints(aconstraints){}
	void integrate(Real timeStep){
		std::vector<Particle>::iterator particleIter;
		std::vector<Constraint>::iterator constraintIter;
		Logger* logger = Logger::getInstance();

		logger->startLogTime("integration");
#ifdef OPTIMIZATION
		#pragma omp parallel for
#endif
		// position based dynamics approach
		for( int i = 0; i < (int)particles.size(); i++){
			Particle& particle = particles[i];

			// record old position
			particle.oldPosition = particle.position;

			Vector3& x0 = particle.position;
			Vector3& v0 = particle.velocity;
			Vector3& accl = particle.acceleration; // acceleration
			particle.newVelocity = v0 + TIME_STEPSIZE * accl;
			// Damp velocities
			particle.newVelocity *= (1.0 - DAMPING);

			particle.newPosition = x0 + particle.newVelocity * TIME_STEPSIZE;
			//particle.newPosition = x0 + particle.newVelocity * TIME_STEPSIZE - 0.5 * TIME_STEPSIZE * TIME_STEPSIZE * accl;
		}
		logger->endLogTime();

		logger->startLogTime("constraint");
		// for each particle (x0->x1), generate collision constraints
					
		// Iteratively projecting constraints (Gauss - Seidel)
		for(int i=0; i<CONSTRAINT_ITERATIONS; i++) // iterate over all constraints several times
		{
#ifdef OPTIMIZATION
		#pragma omp parallel for
#endif
			for( int j = 0; j < (int)constraints.size(); j++){
				constraints[j].satisfyConstraint();
			}
			//for(constraintIter = constraints.begin(); constraintIter != constraints.end(); constraintIter++ )
			//{
			//	(*constraintIter).satisfyConstraint(); // satisfy constraint.
			//}
		}

		logger->endLogTime();

		logger->startLogTime("update");
#ifdef OPTIMIZATION
		#pragma omp parallel for
#endif
		// Update all particles' position and velocity
		for( int i = 0; i < (int)particles.size(); i++){
			Particle& particle = particles[i];
			particle.newVelocity = ( particle.newPosition - particle.position ) / TIME_STEPSIZE;
			//particle.newVelocity = 2 * ( particle.newPosition - particle.position ) / TIME_STEPSIZE - particle.velocity;

			particle.velocity = particle.newVelocity;
			particle.position = particle.newPosition;
			particle.resetAcceleration();
		}
		//for(particleIter = particles.begin(); particleIter != particles.end(); particleIter++)
		//{
		//	Particle& particle = (*particleIter);

		//	particle.newVelocity = ( particle.newPosition - particle.position ) / TIME_STEPSIZE;
		//	//particle.newVelocity = 2 * ( particle.newPosition - particle.position ) / TIME_STEPSIZE - particle.velocity;

		//	particle.velocity = particle.newVelocity;
		//	particle.position = particle.newPosition;
		//	particle.resetAcceleration();
		//}
		logger->endLogTime();
	}
protected:
	std::vector<Particle>& particles;	// particles to integrate
	std::vector<Constraint>& constraints;	// constraints to satisfy
};


#endif